#include "io.h"
#include "defines.h"
#include "fileUtils.h"
#include "coordRef.h"
#include "geometry.h"
#include "utility.h"
#include <algorithm>
#include <eigen3/Eigen/Core>
#include <rapidjson/document.h>
#include <autosdk_hdmap/hdmap_layer_api.h>
#include <iostream>
#include <fstream>

void IO::SaveTrcToSrc(const std::vector<GeoPoint> &geo_points, const std::string &csv_file)
{
    std::ofstream ofs(csv_file);
    ofs.precision(10);
    for (size_t i = 0; i < geo_points.size(); i++)
    {
        ofs << geo_points[i].x << "," << geo_points[i].y << "," << 0.0
            << std::endl;
    }
    ofs.close();
}

GpsBuffer::GpsBuffer()
{
}

GpsBuffer::~GpsBuffer()
{
}

void GpsBuffer::LoadDataFromCsv(const std::string &csv_file)
{
    std::vector<GeoPoint> gps_points;
    std::ifstream ifs(csv_file, std::ios::in);
    std::string s;
    getline(ifs, s);
    while (getline(ifs, s))
    {
        std::vector<std::string> split_str = FileUtils::SpliteStr(s, ",");
        if (split_str.size() != 6)
            continue;

        GeoPoint geo;
        geo.timestamp = std::stol(split_str[0]);
        geo.latitude = std::stod(split_str[1]);
        geo.longitude = std::stod(split_str[2]);
        geo.altitude = 0.0;
        geo.imagename = split_str[4];

        double x, y, z;
        CoordRef::Instance().WGS2GKP(geo.longitude, geo.latitude, geo.altitude, x, y, z);
        geo.x = x;
        geo.y = y;
        geo.z = z;
        gps_points.push_back(geo);
    }

    std::sort(gps_points.begin(), gps_points.end(), [=](const GeoPoint &lhs, const GeoPoint &rhs) -> bool {
        return lhs.timestamp < rhs.timestamp;
    });

    if (gps_points.size() <= 3)
    {
        return;
    }
    for (std::size_t i = 0; i < gps_points.size() - 3; i++)
    {
        GeoPoint a0 = gps_points[i];
        GeoPoint a1 = gps_points[i + 3];
        double delta_t = (a1.timestamp - a0.timestamp) / 1000.0;
        double delta_s = Eigen::Vector2d(a1.x - a0.x, a1.y - a0.y).norm();
        gps_points[i].bearing = std::atan2(a1.y - a0.y, a1.x - a0.x);
        gps_points[i].speed = delta_s / delta_t;
    }

    for (std::size_t i = gps_points.size() - 3; i < gps_points.size(); i++)
    {
        gps_points[i].speed = gps_points[i - 1].speed;
        gps_points[i].bearing = gps_points[i - 1].bearing;
    }

    for(std::size_t i = 0; i < gps_points.size() - 1; i++)
    {
        GeoPoint a0 = gps_points[i];
        GeoPoint a1 = gps_points[i + 1];
        double delta_t = (a1.timestamp - a0.timestamp) / 1000.0;
        double delta_yaw = a1.bearing - a0.bearing;
        //gps_points[i].yawrate = delta_yaw / delta_t;
        gps_points[i].yawrate = 0.0;
    }
    gps_points[gps_points.size() - 1].yawrate = 0.0;

    std::reverse(gps_points.begin(), gps_points.end());
    gps_points_.swap(gps_points);
    std::cout << gps_points_.size() << std::endl;
}

bool GpsBuffer::ReadNext(GeoPoint &gps_point)
{
    if (gps_points_.empty())
        return false;

    gps_point = gps_points_.back();
    gps_points_.pop_back();
    remain_points_.push_back(gps_point);
    return true;
}

void GpsBuffer::Print(DbgFrame *dbg_frame)
{
    std::vector<GeoPoint> total_trc_points;
    total_trc_points.insert(total_trc_points.end(), remain_points_.begin(), remain_points_.end());
    dbg_frame->DrawTrcPoints(total_trc_points, cv::Scalar(0, 0, 255));
}

DlFrameBuffer::DlFrameBuffer(const std::string &dl_lane_path, const std::string &dl_sign_path)
    : dl_lane_path_(dl_lane_path), dl_sign_path_(dl_sign_path)
{
}

DlFrameBuffer::~DlFrameBuffer()
{
}

bool DlFrameBuffer::LoadLaneFromJson(const std::string &json_file_name, const Camera& camera, std::vector<DlLane> &dl_lanes)
{
    std::ifstream in(json_file_name, std::ios::in);
    if (!in.is_open())
    {
        return false;
    }

    std::string json_content((std::istreambuf_iterator<char>(in)), std::istreambuf_iterator<char>());
    rapidjson::Document dom;
    dom.Parse(json_content.c_str());
    if (dom.HasMember("objects") && dom["objects"].IsArray())
    {
        const rapidjson::Value &vals = dom["objects"];
        for (std::size_t i = 0; i < vals.Size(); i++)
        {
            DlLane dl_lane;
            const rapidjson::Value &val = vals[i];
            if (val.HasMember("category") && val["category"].IsString())
            {
                dl_lane.SetColor(val["category"].GetString());
                dl_lane.SetType(val["category"].GetString());
            }
            if (val.HasMember("polygon") && val["polygon"].IsArray())
            {
                const rapidjson::Value &polygon = val["polygon"];
                for (std::size_t j = 0; j < polygon.Size(); j++)
                {
                    dl_lane.polygon.emplace_back(
                        polygon[j][0].GetFloat(),
                        polygon[j][1].GetFloat());
                }
                std::vector<cv::Point2d> undistort_points, cv_polygon;
                for (Eigen::Vector2d v : dl_lane.polygon)
                {
                    cv_polygon.emplace_back(v.x(), v.y());
                }
                cv::undistortPoints(cv_polygon, undistort_points, camera.get_K(), camera.get_D(), cv::noArray(), camera.get_K());
                for(cv::Point2d v : undistort_points)
                {
                    dl_lane.calibrated_polygon.emplace_back(v.x, v.y);
                }
            }
            if(val.HasMember("bbox"))
            {
                double xmin = val["bbox"]["xmin"].GetFloat();
                double ymin = val["bbox"]["ymin"].GetFloat();
                double xmax = val["bbox"]["xmax"].GetFloat();
                double ymax = val["bbox"]["ymax"].GetFloat();
                dl_lane.bBox = envelope2D(xmin, ymin, xmax, ymax);
            }
            if(dl_lane.bBox.Height() > 50)
            {
                dl_lanes.emplace_back(std::move(dl_lane));
            }
        }
    }
    else
    {
        in.close();
        return false;
    }
    in.close();
    return true;
}

bool DlFrameBuffer::LoadSignFromJson(const std::string &json_file_name, const Camera& camera, std::vector<DlSign> &dl_signs)
{
    std::ifstream in(json_file_name, std::ios::in);
    if (!in.is_open())
    {
        return false;
    }

    std::string json_content((std::istreambuf_iterator<char>(in)), std::istreambuf_iterator<char>());
    rapidjson::Document dom;
    dom.Parse(json_content.c_str());
    if (dom.HasMember("objects") && dom["objects"].IsArray())
    {
        const rapidjson::Value &vals = dom["objects"];
        for (std::size_t i = 0; i < vals.Size(); i++)
        {
            DlSign dl_sign;
            const rapidjson::Value &val = vals[i];
            if (val.HasMember("category") && val["category"].IsString())
            {
                dl_sign.category = val["category"].GetString();
            }
            if (val.HasMember("bbox"))
            {
                double xmin = val["bbox"]["xmin"].GetFloat();
                double ymin = val["bbox"]["ymin"].GetFloat();
                double xmax = val["bbox"]["xmax"].GetFloat();
                double ymax = val["bbox"]["ymax"].GetFloat();
                dl_sign.bBox = envelope2D(xmin, ymin, xmax, ymax);

                std::vector<cv::Point2d> undistort_points, cv_polygon;
                cv_polygon.emplace_back(xmin, ymin);
                cv_polygon.emplace_back(xmax, ymax);
                
                cv::undistortPoints(cv_polygon, undistort_points, camera.get_K(), camera.get_D(), cv::noArray(), camera.get_K());
                cv::Point2d lt = undistort_points[0];
                cv::Point2d rb = undistort_points[1];
                dl_sign.calibrated_bbox = envelope2D(lt.x, lt.y, rb.x, rb.y);
            }
            if(dl_sign.bBox.GetArea() > 2500)
            {
                dl_signs.emplace_back(std::move(dl_sign));
            }
        }
    }
    else
    {
        in.close();
        return false;
    }
    in.close();
    RemoveOverlapSigns(dl_signs);
    return true;
}

bool DlFrameBuffer::ReadNext(const GeoPoint &gps_point, const Camera& camera, DlFrame &dl_frame)
{
    std::vector<DlLane> lanes;
    std::vector<DlSign> signs;
    std::string lane_json_file = dl_lane_path_ + FileUtils::base_name_part(gps_point.imagename) + ".json";
    std::string sign_json_file = dl_sign_path_ + FileUtils::base_name_part(gps_point.imagename) + ".json";

    bool res1 = LoadLaneFromJson(lane_json_file, camera, lanes);
    bool res2 = LoadSignFromJson(sign_json_file, camera, signs);

    if (res1 && res2)
    {
        dl_frame.lanes = lanes;
        dl_frame.signs = signs;
        return true;
    }
    return false;
}


void DlFrameBuffer::RemoveOverlapSigns(std::vector<DlSign>& dl_signs)
{
    for (std::size_t i = 0; i < dl_signs.size(); ++i)
    {
        auto &cur_sign = dl_signs[i];
        if (cur_sign.is_overlaypped())
        {
            continue;
        }
        for (size_t j = 0; j < dl_signs.size(); ++j)
        {
            auto &candiated_sign = dl_signs[j];
            if (i == j || candiated_sign.is_overlaypped())
            {
                continue;
            }

            envelope2D intersection_rect = envelope2D::Intersection(cur_sign.calibrated_bbox, candiated_sign.calibrated_bbox);
            float overlapped_ratio = intersection_rect.GetArea() / candiated_sign.calibrated_bbox.GetArea();
            if (overlapped_ratio > 0.9)
            {
                candiated_sign.set_overlaypped();
            }
            else
            {
                envelope2D union_rect = envelope2D::Union(cur_sign.calibrated_bbox, candiated_sign.calibrated_bbox);
                float union_ratio = union_rect.GetArea() / (cur_sign.calibrated_bbox.GetArea() + candiated_sign.calibrated_bbox.GetArea());
                if (union_ratio > 1.0 && union_ratio < 1.05)
                {
                    cur_sign.calibrated_bbox = union_rect;
                    candiated_sign.set_overlaypped();
                }
            }
        }
    }
    std::vector<DlSign> normal_signs;
    for (std::size_t i = 0; i < dl_signs.size(); ++i)
    {
        if(!dl_signs[i].is_overlaypped())
        {
            normal_signs.push_back(dl_signs[i]); 
        }
    }
    dl_signs.swap(normal_signs);
}

hdmap_frame HDMapBuffer::LoadMapDataByPostion(const GeoPoint &gps_point, double range)
{
    hdmap_frame hdmap_f = MAPSDK::HDMapLayer::Instance().SearchByRadius(gps_point, range);
    hdmap_f.hdmap_lanes = Utility::LoadByRange(hdmap_f.hdmap_lanes, gps_point, 100.0);
    return hdmap_f;
}
